#include <i2c.h>
#include <timers.h>
#include <delays.h>
#include "smallsatos.h"
#include "init.h"
#include "ina209.h"
#include "rtc.h"
#include "ecan.h"
#include "int_eeprom.h"
#include "storage.h"
#include "dconfig.h"

/* Fuses */
#pragma config OSC=HS, FCMEN=ON /* Fail-safe clock monitor */, PWRT=ON, WDT=OFF, WDTPS=512, LVP=OFF, DEBUG=OFF
#pragma config EBTR0=OFF, EBTR1=OFF, EBTR2=OFF, EBTR3=OFF, EBTRB=OFF
#pragma config BOREN=OFF /* Brown-out reset voltage 2.8V */ /*IRCIO67*/

/* I2C addresses (TODO check if required - otherwise delete) */
#define CONTROL_IN_RTC 0xA2
#define CONTROL_OUT_RTC 0xA3
#define CONTROL_IN_EEPROM 0xA0
#define CONTROL_OUT_EEPROM 0xA1

/**
 * Operating mode
 */
INT8U mode;

void init ()
{
	int i;

	/* Ports initialisation */
	CMCON = 7; /* Disable the comparator */
	ADCON1 = 0x07; /* All outputs digital */
	CAN_TX_TRIS = OUTPUT;
	CAN_RX_TRIS = INPUT;
	CIOCON = 0x20;
	LED0_TRIS = OUTPUT;
	LED1_TRIS = OUTPUT;
	MCP2551_RS_TRIS = OUTPUT;
	MCP2551_RS = 0;
	USART_RX_TRIS = INPUT;
	USART_TX_TRIS = OUTPUT;
	
	/* Disable global interrupts and enable TIMER0 interrupt */
	INTCON = 0x20;

	/* Enable IDLE mode */
	OSCCONbits.IDLEN = 1;
	
	/* Internal RC oscillator frequency */
	OSCCONbits.IRCF0 = 0; //
	OSCCONbits.IRCF1 = 1; // 4 MHz
	OSCCONbits.IRCF2 = 1; //




	/* Timer0 initialisation */
	OpenTimer0(TIMER_INT_ON & T0_16BIT & T0_SOURCE_INT & T0_PS_1_2);

	/* CAN bus initialisation (see ECAN.def) */
	ECANInitialize();
	MCP2551_RS = 1;

	IPR3bits.RXB0IP = 0;	/* CAN buffer 0 interrupt -> low priority */
	IPR3bits.RXB1IP = 0;	/* CAN buffer 1 interrupt -> low priority */
	PIE3bits.RXB0IE = 0;	/* Enable buffer 0 priority */
	PIE3bits.RXB1IE = 0;	/* Enable buffer 1 priority */

	/* Enable CAN interrupts */
	TXBIE = 0xFF; /* Full TX buffer */
	BIE0 = 0xFF; /* Empty RX buffer */

	/**
	 * Enable CAN bus activity interrupt
	 * This interrupt can wake the CPU from SLEEP or IDLE
	 */
	//PIE3bits.WAKIE = 0;

	/**
	  * I2C initialisation
	  */
	SSPADD = 12; /* Low speed */
	OpenI2C(MASTER, SLEW_OFF);

	/**
	  * RTC initialisation
	  */
	update_system_time(); /* Synchronize SW RTC with HW RTC

	/**
	  * INA209 initialisation
	  */
	for (i=0; i<7; i++) { /* Device ID 0-7 */
		ina209_init(i, 0x099F);
	}
	
	/* Load the operating mode */
	mode = EERead(1020) & EERead(1021) &
		   EERead(1022) & EERead(1023);

	if (mode != PRELAUNCH && mode != ANT_DEPLOYMENT &&
		mode != SAFE && mode != MISSION)
	{
		/* Corrupted operating mode, go back to prelaunch */
		mode = PRELAUNCH;
	}

	/* Refresh the operating mode in EEPROM */
	for (i=EE_MODE; i<(EE_MODE + 4); i++)
	{
		EEWrite(i, mode);
	}

	/* Unmask the global interrupts */
	INTCONbits.GIE = 1;

	/* Unmask high priority interrupts */
	INTCONbits.GIEH = 0;	

	/* Unmask periph. interrupts (CAN...) */
	INTCONbits.PEIE = 1;

	/* Load timers */
	loadTimers();

	LED0=0; LED1=0; /* TODO delete (no LEDs in space) */
}